clc;
clear;
close all;
% 对照vivado project QPSK_IMPLEMENTATION_USING_AD9361进行定点化验证
%% 发射机部分
fs = 10e6; %sample frequency
ps = 2.5e6; %symbol rate
M = 4; %MPSK
samPersymb = fs / ps; %samples per symbol

% 映射mapping
data = [];
% msg = [0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 0, 0, 0, 0, 1, 0, 1, 0, 0];
% msg = [repmat(msg, 1, 400)];
load('data_to_be_send_fpga.mat');
msg = [0, 0, repmat(msg, 1, 10)];
% msg = msg(1:100);

for i = 1:2:length(msg) - 1

    if (msg(i) == 0 && msg(i + 1) == 0) data = [data, 1];
    elseif (msg(i) == 0 && msg(i + 1) == 1) data = [data, sqrt(-1)];
    elseif (msg(i) == 1 && msg(i + 1) == 0) data = [data, -1];
    elseif (msg(i) == 1 && msg(i + 1) == 1) data = [data, -1 * sqrt(-1)];
    end

end

bar(real(data));
% 4倍过采样并成型滤波
rollof = 0.5; %roll-off factor
% 用12bit，也就是12个整数位来表示data，范围为-1~1
% 用16bit，也就是1个符号位+15个小数位来表示根升余弦滤波器的系数，范围为-1~1
rcos_fir = round(1.15 * rcosdesign(rollof, 6, fs / ps, 'sqrt') * 2^15);
% 升余弦滤波时将data扩充为16bit，也就是16个整数位
% 而FIR核将两者相乘最后得到的是17个整数位+15个小数位，范围为-1~1
% 所以最后保留为1个整数位+11个小数位，所以截断最后4个bit，所以除以2^4
data_rcos = upfirdn(data, rcos_fir, samPersymb) / 16;
% 使用Farrow内插实现6倍内插
% 内插之后的结果依然是12bit的data_farrow，1个整数位+11个小数位
data_farrow = floor(farrow_int(fs, 60e6, data_rcos));

% debug用，画图显示rcos滤波以及farrow内插之后的结果
% figure; plot(round(real(data_rcos)), 'rs-');
figure; plot((real(data_farrow)), 'b*-'); title('farrow内插后波形图');

%% 接收机部分
% data_farrow = [zeros(1, 5), data_farrow];
% 降采样不影响数据，所以rx_downsample依然是1+11
rx_downsample = downsample(data_farrow, 6);
% 接收机的匹配滤波，根据小数仿真的结果来看rx_rcos的范围在-2~2之间
% 将rx_downsample(1+11)和系数(1+15)相乘，得到的结果为2+26，由于此时的范围为-2~2
% 需要两个整数位，也就是2+10，所以截断最后16bit
% 此时的rx_rcos是2个整数位+10个小数位
rx_rcos = floor(filter(rcos_fir, 1, rx_downsample) / (2^16));
% rx_rcos = floor(rx_rcos / 2.3);
% debug用
% figure; plot(real(rx_downsample), 'rs-'); title('6倍降采样后波形图');
figure; plot(real(rx_rcos), 'rs-'); title('接收机匹配滤波后波形图');
%% gardner
index_sample = 1;

c1 = 255; %0.0078
c2 = 0;

w = 16384; %0.5
nco = 22938; %0.7
nco_temp = nco(1);
u = 19660; %0.6
farrow_time = 0;
bias = 0;

y = 0;
y_delay_1 = 0;
y_delay_2 = 0;

time_error = 0;
time_error_delay = 0;

err_filter = 0;

y_watch = [];
error_watch = [];
w_watch = [];
w_floor_watch = [];
err_filter_watch = [];
err_filter_fixed_floor_watch = [];
err_filter_fixed_watch = [];
u_watch = [];

while (index_sample < length(rx_rcos) - 2)
    nco_temp(index_sample + 1) = nco(index_sample) - w;

    if (nco_temp(index_sample + 1) > 0)
        nco(index_sample + 1) = nco_temp(index_sample + 1);
    else
        nco(index_sample + 1) = mod(nco_temp(index_sample + 1), 2^15);

        % rx_rcos范围为-2~2，为2个整数位+10个小数位组成
        % 整个farrow内插不改变数据的bit数，所以farrow内插出来的结果依然是-2~2，2+10
        FI1 = floor(0.5 * rx_rcos(index_sample + 2)) - floor(0.5 * rx_rcos(index_sample + 1)) ...
        -floor(0.5 * rx_rcos(index_sample)) + floor(0.5 * rx_rcos(index_sample - 1));
        FI2 = rx_rcos(index_sample + 1) + floor(0.5 * rx_rcos(index_sample + 1)) ...
            -floor(0.5 * rx_rcos(index_sample + 2)) - floor(0.5 * rx_rcos(index_sample)) ...
            - floor(0.5 * rx_rcos(index_sample - 1));
        FI3 = rx_rcos(index_sample);
        %         fprintf('%d,%d,%d,%d,',rx_rcos(index_sample + 2),rx_rcos(index_sample + 1),rx_rcos(index_sample),rx_rcos(index_sample-1));
        temp1 = floor(FI1 * u / (2^15));
        temp2 = floor(temp1 * u / (2^15));
        temp3 = floor(FI2 * u / (2^15));
        [y_delay_2, y_delay_1, y] = deal(y_delay_1, y, temp2 + temp3 + FI3);
        y_watch = [y_watch, y];
        %         fprintf('output is %d\n',y);
        clear temp1 temp2 temp3 FI1 FI2 FI3;

        % 误差计算部分
        farrow_time = farrow_time + 1;

        if (mod(farrow_time, 2) == 0)
            % 在计算误差前将rx_rcos扩充为16bit，也就是6+10，但是此时的rx_rcos的范围为-2~2
            % 则err_real和err_imag都是-4~4的范围，依然可以用6+10一共16bit来表示，但是这里使用了3个整数位
            err_real = real(y) - real(y_delay_2);
            err_imag = imag(y) - imag(y_delay_2);
            % 计算乘法的时候是6+10乘以6+10，所以输出是12+20个bit，根据小数仿真的结果来看
            % 最后输出的time_error的范围在-2~2之间，但是time_error一共16bit，所以这里决定
            % 用3个整数位和13个小数位来表示，所以截断后面7个bit，所以需要除以2^7
            err_mul_real = real(y_delay_1) * err_real;
            err_mul_imag = imag(y_delay_1) * err_imag;

            [time_error_delay, time_error] = deal(time_error, floor(err_mul_real / (2^7)) + floor(err_mul_imag / (2^7)));
            %             fprintf('error now is %d,',time_error);
            clear err_real err_imag err_mul_real err_mul_imag;

            error_watch = [error_watch, time_error];

            % 环路滤波
            % 此处time_error的范围为-2~2，用16个bit，也就是3+13来保存数据
            % 那么err_filter的范围为-4~4，用16个bit，也就是4+12来保存数据
            % 在小数仿真的环路滤波中设置的c1为0.0078(16'b0000_0000_1111_1111)，c1的数据格式为1+15
            % 那么如果将c1和err_filter相乘，结果就是5+27，保留
            err_filter = err_filter + time_error;
            err_filter_fixed_point = err_filter / 2^11 + time_error / (2^9);
            %             fprintf('current err_filter is %d, loop_output is %d\n',...
            %                 err_filter,floor(err_filter_fixed_point));
            w = 16370 + floor(err_filter_fixed_point);
            w_watch = [w_watch, w];

            err_filter_watch = [err_filter_watch, err_filter];
            err_filter_fixed_floor_watch = [err_filter_fixed_floor_watch, floor(err_filter_fixed_point)];
            err_filter_fixed_watch = [err_filter_fixed_watch, err_filter_fixed_point];
        end

        u = floor(nco(index_sample) * 2);
        u_watch = [u_watch, u];
    end

    index_sample = index_sample + 1;
end

close all;
figure; plot(w_watch); title('nco控制字');
% figure; plot(w_floor_watch); title('nco_floor控制字');
figure; plot(nco); title('nco值');
figure; plot(u_watch); title('u值');
figure; plot(err_filter_watch); title('err值');
figure; plot(err_filter_fixed_watch); title('err定点值');
figure; plot(err_filter_fixed_floor_watch); title('err定点floor值');
figure;
subplot(211); plot((0:length(rx_rcos) - 1) / (10e6), real(rx_rcos)); hold on
subplot(211); plot((0:length(y_watch) - 1) / (5e6), real(y_watch), 'rs');
subplot(211); plot((0:length(y_watch(2:2:end)) - 1) / (2.5e6), real(y_watch(2:2:end)), 'b*');
hold off;
subplot(212); plot(error_watch); title(['鉴相误差,c1=', num2str(c1), ',c2=', num2str(c2)]);
